#include "robot_status_server.h"
#include <QJsonDocument>
#include <QJsonParseError>
#include <QJsonObject>
#include <QJsonArray>
#include <QTimer>
#include <memory>
#include <chrono>
#include <thread>
#include <future>
#include <QSemaphore>
#include "../include/seer/SCHeadData.h"

using namespace std::chrono_literals;

/**
* 机器人状态查询ROS2服务节点
*/
RobotStatusService::RobotStatusService() 
     : QObject()
    , rclcpp::Node("robot_status_server")
{
    LOG_INFO("RobotStatusService 初始化开始");

    // 初始化TCP连接和请求管理器
    m_statSocket = new SCStatusTcp(this);
    m_requestManager = new RequestManager(this);
    
    // 声明并获取 seer_ip 参数
    this->declare_parameter<std::string>("seer_ip", "192.168.1.100");
    std::string seer_ip_str;
    this->get_parameter("seer_ip", seer_ip_str);
    m_robotIp = QString::fromStdString(seer_ip_str);

    // 声明并获取 stat_port 参数
    this->declare_parameter<int>("stat_port", SEER_API_PORT_STAT);
    this->get_parameter("stat_port", m_statPort);
    
    // 声明并获取 GPIO ID 参数
    this->declare_parameter<int>("gpio1_id", 0);
    this->declare_parameter<int>("gpio2_id", 1);
    this->get_parameter("gpio1_id", gpio1_id);
    this->get_parameter("gpio2_id", gpio2_id);
        
    LOG_INFO("获取参数完成: seer_ip={}, stat_port={}, gpio1_id={}, gpio2_id={}", 
                 m_robotIp.toStdString(), m_statPort, gpio1_id, gpio2_id);
    
    // 先初始化连接相关组件
    initializeConnections();
    
    // 在构造函数中直接连接信号槽（确保在正确线程中）
    connect(m_statSocket->tcpSocket(), &QTcpSocket::stateChanged, 
            this, &RobotStatusService::handleSocketStateChanged);
    connect(m_statSocket->tcpSocket(), &QTcpSocket::errorOccurred, 
            this, &RobotStatusService::handleSocketError);
    
     // 创建健康检查定时器
    m_healthCheckTimer = new QTimer(this);
    m_healthCheckTimer->setInterval(30000); // 30秒检查一次
    connect(m_healthCheckTimer, &QTimer::timeout, this, &RobotStatusService::healthCheckTimeout);
    m_healthCheckTimer->start();
    LOG_INFO("健康检查定时器已启动，间隔: 30秒");

    // 创建服务
    service_ = this->create_service<rms_pkg::srv::RobotStatus>(
        "get_robot_status",
        std::bind(&RobotStatusService::handle_service_request, this,
                 std::placeholders::_1, std::placeholders::_2));
    
    LOG_DEBUG("SRC2000 状态查询服务已启动（查询电量、位置、导航状态、当前地图名称、导航目标点、导航状态）.");
    
    // 初始化缓存超时时间
    cachedStatus_.cacheTimeoutMs = 1000;
}

/**
* @brief 初始化robot连接
*/
void RobotStatusService::initializeConnections()
{
    LOG_INFO("初始化机器人连接");
    
    // 创建重连定时器
    m_reconnectTimer = new QTimer(this);
    m_reconnectTimer->setInterval(5000); // 5秒重连一次
    connect(m_reconnectTimer, &QTimer::timeout, this, &RobotStatusService::reconnectTimerTimeout);
    
    // 尝试连接机器人
    connectToRobot();
    
    // 启动重连定时器
    m_reconnectTimer->start();
    LOG_INFO("重连定时器已启动，间隔: 5秒");
}

/**
* @brief 尝试连接机器人
*/
void RobotStatusService::connectToRobot()
{
    // 检查当前是否已经连接，如果已连接则不执行任何操作
    if (m_statSocket && m_statSocket->tcpSocket() && 
        m_statSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState) {
        LOG_DEBUG("机器人已经连接，无需重新连接");
        // 当连接成功时，重置重连尝试次数
        resetReconnectAttempts();
        return;
    }
    
    // 如果达到最大重连次数，则使用较长的延迟
    if (m_reconnectAttempts >= MAX_RECONNECT_ATTEMPTS) {
        static int connectCount = 0;
        if (++connectCount % 3 != 0) { // 每3次尝试才真正连接一次
            LOG_DEBUG("达到最大重连次数，跳过本次连接尝试");
            return;
        }
    }
    
    LOG_DEBUG("尝试连接到机器人 - IP: {}, 状态端口: {}", 
                  m_robotIp.toStdString(), m_statPort);
    
    // 连接状态端口
    int result = m_statSocket->connectHost(m_robotIp, m_statPort);
    if (result == 0) {
        LOG_INFO("状态端口连接请求已发送");
    } else {
        LOG_WARN("状态端口连接请求发送失败或关闭了现有连接");
    }
}

/**
* 状态端口状态改变处理函数
*/
void RobotStatusService::handleSocketStateChanged(QAbstractSocket::SocketState state)
{
    switch (state) {
        case QAbstractSocket::UnconnectedState:
            LOG_DEBUG("套接字状态: 未连接");
            break;
        case QAbstractSocket::HostLookupState:
            LOG_DEBUG("套接字状态: 正在查找主机");
            break;
        case QAbstractSocket::ConnectingState:
            LOG_DEBUG("套接字状态: 正在连接");
            break;
        case QAbstractSocket::ConnectedState:
            LOG_INFO("套接字状态: 已连接");
            // 当连接成功时，重置重连尝试次数
            resetReconnectAttempts();
            m_wasConnected = true;
            // 连接成功时重启重连定时器，避免不必要的重连尝试
            if (m_reconnectTimer && m_reconnectTimer->isActive()) {
                m_reconnectTimer->stop();
                m_reconnectTimer->start();
                LOG_DEBUG("连接成功，重置重连定时器");
            }
            break;
        case QAbstractSocket::BoundState:
            LOG_DEBUG("套接字状态: 已绑定");
            break;
        case QAbstractSocket::ListeningState:
            LOG_DEBUG("套接字状态: 监听中");
            break;
        case QAbstractSocket::ClosingState:
            LOG_DEBUG("套接字状态: 正在关闭");
            m_wasConnected = false;
            break;
    }
}

/**
* 套接字错误处理
*/
void RobotStatusService::handleSocketError(QAbstractSocket::SocketError error)
{
    LOG_ERROR("套接字错误: {} - 检测到连接问题，计划重新连接", static_cast<int>(error));
    
    // 只有在未达到最大重连次数时才安排重连
    if (m_reconnectAttempts < MAX_RECONNECT_ATTEMPTS) {
        scheduleReconnect(); // 使用改进的重连机制
    } else {
        LOG_ERROR("已达到最大重连次数 {}，不再安排重连", MAX_RECONNECT_ATTEMPTS);
        // 即使达到最大重连次数，也要定期尝试连接以检查网络是否恢复
        static int errorCount = 0;
        if (++errorCount % 10 == 0) { // 每10次错误尝试一次连接
            LOG_INFO("达到最大重连次数，但定期尝试连接以检查网络是否恢复");
            connectToRobot();
        }
    }
}

/**
 * @brief 重连定时器超时处理函数
 */
void RobotStatusService::reconnectTimerTimeout()
{
    
    updateConnectionStatus();
    
    // 只有在连接断开时才进行重连检查
    if (m_statSocket->tcpSocket()->state() != QAbstractSocket::ConnectedState) {
        LOG_DEBUG("状态连接断开，需要重连");
        if (m_reconnectAttempts < MAX_RECONNECT_ATTEMPTS) {
            LOG_INFO("检测到连接断开，尝试重新连接");
            scheduleReconnect();
        } else {
            LOG_ERROR("达到最大重连次数 {}，暂停重连，但仍会定期检查连接状态", MAX_RECONNECT_ATTEMPTS);
            // 即使达到最大重连次数，也要定期尝试连接以检查网络是否恢复
            static int checkCount = 0;
            if (++checkCount % 3 == 0) { // 每3次定时器触发尝试一次连接（约15秒）
                LOG_INFO("达到最大重连次数，但定期尝试连接以检查网络是否恢复");
                connectToRobot();
            }
        }
    }
    // 如果连接正常，则不需要做任何事情，重连定时器会自动继续运行
}

/**
 * @brief 处理机器人状态查询服务请求
 * @param request 服务请求参数，包含查询类型
 * @param response 服务响应，包含机器人的状态信息，如位置、电池电量、导航状态等
 * 
 * 该函数负责处理来自客户端的机器人状态查询请求。它会检查与机器人的连接状态，
 * 如果连接正常，则根据请求的查询类型获取相应的机器人状态信息，并将这些
 * 信息填充到响应中返回给客户端。
 */
void RobotStatusService::handle_service_request(
    const std::shared_ptr<rms_pkg::srv::RobotStatus::Request> request,
    const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response)
{
    LOG_DEBUG("收到查询机器人状态服务请求: {}", request->query_type);
    
    // 检查连接状态
    if (!isConnected()) {
        response->success = false;
        response->message = "Not connected to robot";
        LOG_WARN("Robot not connected!Automatic reconnection!");
        return;
    }
    
    // 检查缓存（仅对查询所有信息时使用）
    if (request->query_type == rms_pkg::srv::RobotStatus::Request::QUERY_SEER_STATUS_INFO_TO_HEARTBEAT && isCacheValid()) {
        *response = cachedStatus_.status;
        response->message = "Successfully retrieved robot status (from cache)";
        LOG_DEBUG("Using cached robot status");
        return;
    }
    
    bool operation_success = false;
    
    // 根据查询类型执行相应的操作
    switch (request->query_type) {
        case rms_pkg::srv::RobotStatus::Request::QUERY_SEER_STATUS_INFO_TO_HEARTBEAT:
            // 并行发送请求获取最新数据
            operation_success = getRobotStatusInfoToHeartbeat(response);
            
            LOG_DEBUG("Current data: battery={}%, pos=({},{},{}), navigation_state={}, target={}",
                           response->battery, response->x, response->y, response->angle,
                           response->navigation_state, response->current_target);
            
            if (operation_success) {
                response->success = true;
                response->message = "Successfully retrieved all robot status";
                updateCache(response); // 更新缓存
                LOG_DEBUG("Successfully retrieved all robot status");
            } else {
                // 检查现有数据是否有效
                bool battery_valid = response->battery >= 0;
                bool position_valid = (response->x != 0.0f) || (response->y != 0.0f) || (response->angle != 0.0f);
                bool nav_valid = !response->navigation_state.empty();
                
                response->success = battery_valid && position_valid && nav_valid;
                if (response->success) {
                    response->message = "Using cached robot status";
                    LOG_DEBUG("Using cached robot status");
                } else {
                    response->message = "Failed to retrieve robot status information";
                    LOG_WARN("Failed to retrieve robot status information");
                }
            }
        break;
            
        case rms_pkg::srv::RobotStatus::Request::QUERY_NAVIGATION_ONLY:
            {
                operation_success = getNavigationStateInfo(response);
                if (operation_success) {
                    response->success = true;
                    response->message = "Successfully retrieved navigation status";
                    LOG_DEBUG("Successfully retrieved navigation status: {} with target {}",
                                  response->navigation_state, response->current_target);
                } else {
                    response->success = false;
                    response->message = "Failed to retrieve navigation status information";
                    LOG_WARN("Failed to retrieve navigation status information");
                }
            }
            break;
            
        case rms_pkg::srv::RobotStatus::Request::QUERY_POSITION_ONLY:
            {
                operation_success = getPositionInfo(response);
                if (operation_success) {
                    response->success = true;
                    response->message = "Successfully retrieved position";
                    LOG_DEBUG("Successfully retrieved position: ({},{},{})",
                                  response->x, response->y, response->angle);
                } else {
                    response->success = false;
                    response->message = "Failed to retrieve position information";
                    LOG_WARN("Failed to retrieve position information");
                }
            }
            break;
            
        case rms_pkg::srv::RobotStatus::Request::QUERY_BATTERY_ONLY:
            {
                operation_success = getBatteryInfo(response);
                if (operation_success) {
                    response->success = true;
                    response->message = "Successfully retrieved battery level";
                    LOG_DEBUG("Successfully retrieved battery level: {}%", response->battery);
                } else {
                    response->success = false;
                    response->message = "Failed to retrieve battery information";
                    LOG_WARN("Failed to retrieve battery information");
                }
            }
            break;

        case rms_pkg::srv::RobotStatus::Request::QUERY_MAP_NAME_ONLY:
            {
                LOG_DEBUG("Processing QUERY_MAP_NAME_ONLY request");
                operation_success = getCurrentMapInfo(response);
                LOG_DEBUG("getCurrentMapInfo returned: {}", operation_success);
                if (operation_success) {
                    response->success = true;
                    response->message = "Successfully retrieved map name";
                    LOG_DEBUG("Successfully retrieved map name: {}", response->current_map);
                } else {
                    response->success = false;
                    response->message = "Failed to retrieve map name";
                    LOG_WARN("Failed to retrieve map name");
                }
                LOG_DEBUG("Final response - success: {}, message: {}, current_map: {}", 
                              response->success, response->message, response->current_map);
            }
            break;
        
        case rms_pkg::srv::RobotStatus::Request::QUERY_CONTROL_AUTHORITY_ONLY:
            {
                LOG_DEBUG("Processing QUERY_CONTROL_AUTHORITY_ONLY request");
                operation_success = getControlAuthorityInfo(response);
                LOG_DEBUG("getControlAuthorityInfo returned: {}", operation_success);
                if (operation_success) {
                    response->success = true;
                    response->message = "Successfully retrieved control authority";
                    LOG_DEBUG("Successfully retrieved control authority: has_control={}, owner={}", 
                                  response->has_control_authority, response->control_owner);
                } else {
                    response->success = false;
                    response->message = "Failed to retrieve control authority";
                    LOG_WARN("Failed to retrieve control authority");
                }
                LOG_DEBUG("Final response - success: {}, message: {}, has_control: {}, owner: {}", 
                              response->success, response->message, response->has_control_authority, response->control_owner);
            }
            break;
        case rms_pkg::srv::RobotStatus::Request::QUERY_SEER_STATUS_INFO_TO_TASK:
            {
                LOG_DEBUG("Processing QUERY_SEER_STATUS_INFO_TO_TASK request");
                operation_success = getRobotStatusInfoToTask(response);  //查询导航状态和控制权
                LOG_DEBUG("getRobotStatusInfoToTask returned: {}", operation_success);
                if (operation_success) {
                    response->success = true;
                    response->message = "Successfully retrieved navigation and control authority";
                    LOG_DEBUG("Successfully retrieved navigation_state: {} and control authority: has_control={}, owner={}, gpio1_state={}, gpio2_state={}", 
                                  response->navigation_state, response->has_control_authority, response->control_owner, response->gpio1_state, response->gpio2_state);
                } else {
                    response->success = false;
                    response->message = "Failed to retrieve navigation and control authority";
                    LOG_WARN("Failed to retrieve navigation and control authority");
                }
                LOG_DEBUG("Final response - success: {}, message: {}, nav_state: {}, has_control: {}, owner: {}, gpio1_state: {}, gpio2_state: {}", 
                              response->success, response->message, response->navigation_state, response->has_control_authority, response->control_owner, response->gpio1_state, response->gpio2_state);
            }
            break;
        default:
            response->success = false;
            response->message = "Invalid query type";
            LOG_WARN("Invalid query type: {}", request->query_type);
            break;
    }
}

/**
* 检查连接状态
*/
bool RobotStatusService::isConnected() const {
    if (!m_statSocket || !m_statSocket->tcpSocket()) {
        LOG_DEBUG("套接字对象为空");
        return false;
    }
    
    QAbstractSocket::SocketState state = m_statSocket->tcpSocket()->state();
    LOG_DEBUG("检查连接状态: {}", static_cast<int>(state));
    return state == QAbstractSocket::ConnectedState;
}

/**
* 获取电量信息(1007)
*/
bool RobotStatusService::getBatteryInfo(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response) {
    return sendRequestAndHandleResponse(CMD_STATE_GET_BATTERY, response);
}

/**
* 获取机器人位置信息(1004)
*/
bool RobotStatusService::getPositionInfo(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response) {
    return sendRequestAndHandleResponse(CMD_STATE_GET_POSITION, response);
}

/**
* 获取导航状态信息(1020)
*/
bool RobotStatusService::getNavigationStateInfo(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response) {
    return sendRequestAndHandleResponse(CMD_STATE_GET_NAVIGATIONAL_STATE, response);
}

/**
 * @brief 获取当前地图信息
 * @param response 服务响应对象
 * @return 操作是否成功
 */
bool RobotStatusService::getCurrentMapInfo(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response) {
    return sendRequestAndHandleResponse(CMD_MAP_GET_CURRENT, response);
}

/**
 * @brief 获取控制权信息(1060)
 */
bool RobotStatusService::getControlAuthorityInfo(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response) {
    return sendRequestAndHandleResponse(CMD_STATE_GET_CONTROL_AUTHORITY, response);
}

/**
 * @brief 发送请求并处理响应
 * 
 * 该函数通过Qt事件循环机制发送请求并等待响应结果，具有超时处理功能。
 * 使用异步回调方式处理响应数据，并在处理完成后退出事件循环。
 * 
 * @param cmdId 命令ID，用于标识请求类型
 * @param response 响应数据的共享指针，用于存储处理后的响应结果
 * @return bool 操作成功返回true，失败返回false（包括超时或处理异常情况）
 */
bool RobotStatusService::sendRequestAndHandleResponse(
    int cmdId,
    std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response)
{
    LOG_DEBUG("开始发送请求 - 命令ID: {}", cmdId);

    // 使用Qt的事件循环等待异步操作完成
    QEventLoop loop;
    bool operationResult = false;
    bool operationCompleted = false;
    bool timeoutOccurred = false;

    auto callback = [this, &operationResult, &operationCompleted, &loop, response, cmdId](const QByteArray& resp) {
        try {
            handleOperationCompleted(cmdId, resp, response);
            operationResult = true;
            operationCompleted = true;
            // 确保在主线程中退出事件循环
            QMetaObject::invokeMethod(&loop, "quit", Qt::QueuedConnection);
        } catch (const std::exception& e) {
            LOG_ERROR("处理响应时发生异常: {}", e.what());
            operationCompleted = true;
            QMetaObject::invokeMethod(&loop, "quit", Qt::QueuedConnection);
        }
    };

    // 使用统一的超时时间
    const int timeoutMs = 5000;
    
    // 设置超时处理
    QTimer timeoutTimer;
    timeoutTimer.setSingleShot(true);
    QObject::connect(&timeoutTimer, &QTimer::timeout, [&loop, &timeoutOccurred]() {
        timeoutOccurred = true;
        loop.quit();
    });
    timeoutTimer.start(timeoutMs);
    
    m_requestManager->sendRequest(m_statSocket, cmdId, QByteArray(), callback, timeoutMs);

    // 运行事件循环直到操作完成或超时
    loop.exec();

    if (timeoutOccurred) {
        LOG_ERROR("请求超时 - 命令ID: {}", cmdId);
        return false;
    }

    if (!operationCompleted) {
        LOG_ERROR("操作未完成 - 命令ID: {}", cmdId);
        return false;
    }

    return operationResult;
}

/**
 * @brief 处理操作完成的回调函数
 * 
 * 根据命令ID解析响应数据，并更新对应的机器人状态信息。
 * 支持的命令包括获取电池电量、位置信息和导航状态等。
 * 
 * @param cmdId 命令ID，用于区分不同的操作类型
 * @param resp 响应数据，以QByteArray格式存储的JSON数据
 * @param response 机器人状态服务的响应对象，用于存储解析后的状态信息
 * @return bool 处理成功返回true，失败返回false
 */
bool RobotStatusService::handleOperationCompleted(
    int cmdId,
    const QByteArray& resp,
    std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response)
{
    if (resp.isEmpty()) {
        LOG_WARN("响应数据为空 - 命令ID: {}", cmdId);
        return false;
    }

    QMutexLocker locker(&m_dataMutex); // 加锁保护共享数据
    QJsonParseError parseError;
    QJsonDocument doc = QJsonDocument::fromJson(resp, &parseError);
    if (parseError.error != QJsonParseError::NoError) {
        LOG_ERROR("JSON解析失败: {}", parseError.errorString().toStdString());
        return false;
    }
    QJsonObject obj = doc.object();
    locker.unlock();
    
    bool success = false;
    
    // 根据命令ID处理不同的响应数据
    switch (cmdId) {
        case CMD_STATE_GET_BATTERY: {
            // 解析电池电量信息
            double battery_level = obj["battery_level"].toDouble(-1.0);
            if (battery_level >= 0) {
                response->battery = static_cast<int8_t>(battery_level * 100);
                LOG_DEBUG("成功获取电池信息: {}%", battery_level);
                success = true;
            } else {
                LOG_WARN("无效的电池电量值");
            }
            break;
        }
        case CMD_STATE_GET_POSITION: {
            // 解析位置信息
            double x = obj["x"].toDouble(0.0);
            double y = obj["y"].toDouble(0.0);
            double angle = obj["angle"].toDouble(0.0);
            QString currentStation = obj["current_station"].toString();
            QString lastStation = obj["last_station"].toString();
            
            response->x = static_cast<float>(x);
            response->y = static_cast<float>(y);
            response->angle = static_cast<float>(angle);
            
            // 如果current_station不为空，则赋值给response->current_target
            if (!currentStation.isEmpty()) {
                response->current_station = currentStation.toStdString();
            }
            
            // 如果last_station不为空，则赋值给response->last_station
            if (!lastStation.isEmpty()) {
                response->last_station = lastStation.toStdString();
            }
            
            LOG_DEBUG("成功获取位置信息: x={}, y={}, angle={} lastStation={} currentStation={}", x, y, angle,  lastStation.toStdString(), currentStation.toStdString());
            success = true; // 假设只要解析了就认为成功
            break;
        }
        case CMD_STATE_GET_NAVIGATIONAL_STATE: {
            // 解析导航状态信息
            int taskStatus = obj["task_status"].toInt(-1);
            QString newState = getStateString(taskStatus);
            QString newTarget = obj["target_id"].toString();
            response->navigation_state = newState.toStdString();
            response->current_target = newTarget.toStdString();
            LOG_DEBUG("成功获取导航状态信息: status={}, currentTarget={}", newState.toStdString(), newTarget.toStdString());
            success = true; // 假设只要解析了就认为成功
            break;
        }
        case CMD_MAP_GET_CURRENT: {
            // 解析当前地图信息
            QString currentMap = obj["current_map"].toString();
            response->current_map = currentMap.toStdString();
            LOG_DEBUG("成功获取当前地图信息: {}", currentMap.toStdString());
            success = true;
            break;
        }
        case CMD_STATE_GET_CONTROL_AUTHORITY: {
            // 解析控制权信息
            bool locked = obj["locked"].toBool();
            QString owner = obj["nick_name"].toString();
            
            // 如果locked为false，表示控制权为自己所有;如果locked为true，表示控制权属于其他客户端;
            response->has_control_authority = !locked;
            response->control_owner = owner.toStdString();
            
            LOG_DEBUG("成功获取控制权信息: owner={}", 
                          response->has_control_authority, owner.toStdString());
            success = true;
            break;
        }
        case CMD_GET_GPIO_STATUS: {
            // 解析GPIO状态信息
            QJsonArray diArray = obj["DI"].toArray();
            
            // 遍历DI数组，查找配置文件中指定的GPIO ID状态
            for (const auto& diValue : diArray) {
                QJsonObject diObj = diValue.toObject();
                int id = diObj["id"].toInt(-1);
                bool status = diObj["status"].toBool(false);
                
                // 根据配置的ID设置对应的GPIO状态
                if (id == gpio1_id) {
                    response->gpio1_state = status;
                } else if (id == gpio2_id) {
                    response->gpio2_state = status;
                }
            }
            
            LOG_DEBUG("成功获取GPIO状态信息: gpio1_id={}, gpio1_state={}, gpio2_id={}, gpio2_state={}", 
                      gpio1_id, response->gpio1_state, gpio2_id, response->gpio2_state);
            success = true;
            break;
        }
        default:
            LOG_WARN("未知命令ID: {}", cmdId);
    }
    
    return success;
}

/**
* 获取当前导航状态
*/
QString RobotStatusService::getStateString(int taskStatus) {
    switch (taskStatus) {
        case 0: 
            LOG_DEBUG("任务状态: NONE");
            return "NONE";
        case 2: 
            LOG_DEBUG("任务状态: RUNNING");
            return "RUNNING";
        case 3: 
            LOG_DEBUG("任务状态: SUSPENDED");
            return "SUSPENDED";
        case 4: 
            LOG_DEBUG("任务状态: COMPLETED");
            return "COMPLETED";
        case 5: 
            LOG_DEBUG("任务状态: FAILED");
            return "FAILED";
        case 6: 
            LOG_DEBUG("任务状态: CANCELED");
            return "CANCELED";
        default: 
            LOG_DEBUG("任务状态: UNKNOWN ({})", taskStatus);
            return "UNKNOWN";
    }
}

/**
 * @brief 健康检查定时器超时处理函数
 * 
 * 此函数作为定时器超时的槽函数，用于定期执行健康检查。
 * 它会记录健康检查的执行日志并调用实际的健康检查函数。
 * 该函数通过定时器机制定期触发，以确保机器人的连接状态得到持续监控。
 */
void RobotStatusService::healthCheckTimeout()
{
    healthCheck();
}

/**
 * @brief 执行机器人连接健康检查
 * 
 * 该函数定期检查与机器人的连接状态，在连接断开时安排重连。
 * 使用原子变量确保同一时间只有一个健康检查在进行，避免并发问题。
 * 健康检查只在空闲时执行，不会干扰正在进行的请求。
 */
void RobotStatusService::healthCheck() {
    // 只在空闲时进行健康检查，避免干扰正在进行的请求
    static std::atomic<bool> healthCheckInProgress{false};
    
    if (healthCheckInProgress.exchange(true)) {
        return; // 已有健康检查在进行
    }
    
    // 执行轻量级健康检查
    if (!isConnected()) {
        LOG_DEBUG("健康检查发现连接断开");
        // 只有在重连次数未达到上限时才尝试重连
        if (m_reconnectAttempts < MAX_RECONNECT_ATTEMPTS) {
            LOG_DEBUG("计划重新连接");
            scheduleReconnect();
        } else {
            // 即使达到最大重连次数，也要定期尝试连接
            static int checkCount = 0;
            if (++checkCount % 3 == 0) { // 每3次健康检查尝试一次连接（约1.5分钟）
                LOG_INFO("达到最大重连次数，但定期尝试连接以检查网络是否恢复");
                connectToRobot();
                // 这种定期尝试不计入重连次数，避免无限增长
            }
        }
    } else {
        // 如果连接正常，确保重连计数器被重置
        if (m_reconnectAttempts > 0) {
            LOG_DEBUG("连接正常，重置重连计数器");
            resetReconnectAttempts();
        }
    }
    
    healthCheckInProgress = false;
}

/**
 * @brief 更新机器人连接状态
 * 
 * 检查TCP套接字的连接状态，并根据当前连接状态与之前连接状态的比较
 * 来确定是否发生了连接建立或断开事件。当检测到状态变化时，会记录相应的
 * 日志信息并执行相应的操作（如重置重连尝试次数）。
 * 
 * 连接恢复时：
 * - 记录信息日志
 * - 更新连接状态标志
 * - 重置重连尝试次数
 * 
 * 连接断开时：
 * - 记录警告日志
 * - 更新连接状态标志
 */
void RobotStatusService::updateConnectionStatus()
{
    bool connected = (m_statSocket->tcpSocket()->state() == QAbstractSocket::ConnectedState);
    
    if (connected && !m_wasConnected) {
        LOG_INFO("连接已恢复");
        m_wasConnected = true;
        resetReconnectAttempts(); // 重置重连次数
    } else if (!connected && m_wasConnected) {
        LOG_WARN("连接断开，开始重连流程");
        m_wasConnected = false;
    }
}

/**
 * @brief 使用指数退避算法调度机器人重连
 * 
 * 该函数实现了一个带有抖动的指数退避重连机制，防止在网络不稳定时频繁重连。
 * 重连延迟时间随着重连次数增加而指数增长，并添加随机抖动避免同步重试。
 * 当重连次数达到最大限制时，将使用固定间隔继续尝试重连。
 * 
 * @note 重连延迟范围在1秒到30秒之间，包含±20%的随机抖动
 */
void RobotStatusService::scheduleReconnect()
{
    // 指数退避算法 + 抖动
    int baseDelay = 1000; // 基础延迟1秒
    int maxDelay = 30000; // 最大延迟30秒
    int delay = std::min(baseDelay * (1 << m_reconnectAttempts), maxDelay);
    
    // 添加抖动 (±20%)
    int jitter = (rand() % (delay / 5)) - (delay / 10);
    delay += jitter;
    delay = std::max(1000, delay); // 最小1秒
    
    // 只有在安排重连时才增加重连尝试次数
    m_reconnectAttempts++;
    LOG_INFO("计划在 {} ms 后进行第 {} 次重连", delay, m_reconnectAttempts);
    
    QTimer::singleShot(delay, this, &RobotStatusService::connectToRobot);
}

// 重置重连尝试次数
void RobotStatusService::resetReconnectAttempts()
{
    m_reconnectAttempts = 0;
}

/**
 * @brief 检查缓存是否有效
 * 
 * 通过比较缓存时间和当前时间的差值与缓存超时时间来判断缓存是否仍然有效。
 * 如果缓存时间不为空且距离当前时间的毫秒数小于缓存超时毫秒数，则认为缓存有效。
 * 
 * @return bool 缓存有效返回true，否则返回false
 */
bool RobotStatusService::isCacheValid() const {
    return !cachedStatus_.timestamp.isNull() && 
           cachedStatus_.timestamp.msecsTo(QDateTime::currentDateTime()) < cachedStatus_.cacheTimeoutMs;
}

/**
 * @brief 更新缓存状态
 * 
 * 将传入的响应数据保存到缓存中，并更新缓存时间戳为当前时间。
 * 
 * @param response 包含最新机器人状态信息的响应数据指针
 */
void RobotStatusService::updateCache(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response) {
    cachedStatus_.status = *response;
    cachedStatus_.timestamp = QDateTime::currentDateTime();
}

/**
 * @brief 并行获取导航状态和控制权所有者和位置信息和GPIO状态返回给任务模块
 * 
 * 该函数通过并行发送请求来获取机器人的导航状态、控制权所有者、位置信息、GPIO状态，
 * 并等待所有请求完成或超时。使用Qt事件循环来管理异步请求。
 * 
 * @param response 用于存储机器人状态信息的响应对象
 * @return bool 如果所有请求都成功完成则返回true，否则返回false
 */
bool RobotStatusService::getRobotStatusInfoToTask(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response)
{
    LOG_DEBUG("开始并行获取导航状态和控制权限信息");

    // 使用事件循环等待所有请求完成
    QEventLoop loop;
    QTimer timeoutTimer;
    timeoutTimer.setSingleShot(true);
    
    // 跟踪请求状态
    struct RequestStatus {
        bool completed = false;
        bool success = false;
    };
    
    std::map<int, RequestStatus> requestStatuses = {
        {CMD_STATE_GET_NAVIGATIONAL_STATE, {}},
        {CMD_STATE_GET_CONTROL_AUTHORITY, {}},
        {CMD_STATE_GET_POSITION, {}},  // 位置信息请求
        {CMD_GET_GPIO_STATUS, {}}      // GPIO状态请求
    };
    
    int completedRequests = 0;
    const int totalRequests = 4;  // 更新请求数量
    bool timeoutOccurred = false;

    // 定义通用回调函数
    auto createCallback = [this, &requestStatuses, &completedRequests, &loop, &totalRequests, response]
                          (int cmdId, const QByteArray& resp) {
        try {
            handleOperationCompleted(cmdId, resp, response);
            requestStatuses[cmdId].completed = true;
            requestStatuses[cmdId].success = true;
            completedRequests++;
            
            // 所有请求完成时退出循环
            if (completedRequests >= totalRequests) {
                QMetaObject::invokeMethod(&loop, "quit", Qt::QueuedConnection);
            }
        } catch (const std::exception& e) {
            LOG_ERROR("处理响应时发生异常: {}", e.what());
            requestStatuses[cmdId].completed = true;
            requestStatuses[cmdId].success = false;
            completedRequests++;
            
            if (completedRequests >= totalRequests) {
                QMetaObject::invokeMethod(&loop, "quit", Qt::QueuedConnection);
            }
        }
    };

    // 发送所有请求
    auto navCallback = [createCallback](const QByteArray& resp) {
        createCallback(CMD_STATE_GET_NAVIGATIONAL_STATE, resp);
    };
    
    auto controlCallback = [createCallback](const QByteArray& resp) {
        createCallback(CMD_STATE_GET_CONTROL_AUTHORITY, resp);
    };
    
    // 位置信息请求回调
    auto positionCallback = [createCallback](const QByteArray& resp) {
        createCallback(CMD_STATE_GET_POSITION, resp);
    };

    // GPIO状态请求回调
    auto gpioCallback = [createCallback](const QByteArray& resp) {
        createCallback(CMD_GET_GPIO_STATUS, resp);
    };

    m_requestManager->sendRequest(m_statSocket, CMD_STATE_GET_NAVIGATIONAL_STATE, QByteArray(), navCallback, 5000);
    m_requestManager->sendRequest(m_statSocket, CMD_STATE_GET_CONTROL_AUTHORITY, QByteArray(), controlCallback, 5000);
    // 位置信息请求
    m_requestManager->sendRequest(m_statSocket, CMD_STATE_GET_POSITION, QByteArray(), positionCallback, 5000);
    // GPIO状态请求
    m_requestManager->sendRequest(m_statSocket, CMD_GET_GPIO_STATUS, QByteArray(), gpioCallback, 5000);

    // 设置超时
    QObject::connect(&timeoutTimer, &QTimer::timeout, [&loop, &timeoutOccurred]() {
        timeoutOccurred = true;
        loop.quit();
    });
    timeoutTimer.start(5000);

    // 等待所有请求完成或超时
    loop.exec();

    if (timeoutOccurred) {
        LOG_ERROR("请求超时");
        return false;
    }

    // 检查所有请求是否成功完成
    bool allSuccess = true;
    for (const auto& pair : requestStatuses) {
        if (!pair.second.completed || !pair.second.success) {
            allSuccess = false;
            LOG_DEBUG("请求未完成或失败 - 命令ID: {}", pair.first);
        }
    }

    LOG_DEBUG("导航状态和控制权限信息获取完成 - 结果: {}", allSuccess);
    return allSuccess;
}

/**
 * @brief 并行获取机器人电量、位置、导航状态返回给心跳模块
 * 
 * 该函数通过并行发送多个请求来获取机器人的电池状态、位置状态、导航状态，
 * 并等待所有请求完成或超时。使用Qt事件循环来管理异步请求。
 * 
 * @param response 用于存储机器人状态信息的响应对象
 * @return bool 如果所有请求都成功完成则返回true，否则返回false
 */
bool RobotStatusService::getRobotStatusInfoToHeartbeat(const std::shared_ptr<rms_pkg::srv::RobotStatus::Response> response)
{
    LOG_DEBUG("开始并行获取所有机器人状态");

    // 使用事件循环等待所有请求完成
    QEventLoop loop;
    QTimer timeoutTimer;
    timeoutTimer.setSingleShot(true);
    
    // 跟踪请求状态
    struct RequestStatus {
        bool completed = false;
        bool success = false;
    };
    
    std::map<int, RequestStatus> requestStatuses = {
        {CMD_STATE_GET_BATTERY, {}},
        {CMD_STATE_GET_POSITION, {}},
        {CMD_STATE_GET_NAVIGATIONAL_STATE, {}}
    };
    
    int completedRequests = 0;
    const int totalRequests = 3;
    bool timeoutOccurred = false;

    // 定义通用回调函数
    auto createCallback = [this, &requestStatuses, &completedRequests, &loop, &totalRequests, response]
                          (int cmdId, const QByteArray& resp) {
        try {
            handleOperationCompleted(cmdId, resp, response);
            requestStatuses[cmdId].completed = true;
            requestStatuses[cmdId].success = true;
            completedRequests++;
            
            // 所有请求完成时退出循环
            if (completedRequests >= totalRequests) {
                QMetaObject::invokeMethod(&loop, "quit", Qt::QueuedConnection);
            }
        } catch (const std::exception& e) {
            LOG_ERROR("处理响应时发生异常: {}", e.what());
            requestStatuses[cmdId].completed = true;
            requestStatuses[cmdId].success = false;
            completedRequests++;
            
            if (completedRequests >= totalRequests) {
                QMetaObject::invokeMethod(&loop, "quit", Qt::QueuedConnection);
            }
        }
    };

    // 发送所有请求
    auto batteryCallback = [createCallback](const QByteArray& resp) {
        createCallback(CMD_STATE_GET_BATTERY, resp);
    };
    
    auto positionCallback = [createCallback](const QByteArray& resp) {
        createCallback(CMD_STATE_GET_POSITION, resp);
    };
    
    auto navCallback = [createCallback](const QByteArray& resp) {
        createCallback(CMD_STATE_GET_NAVIGATIONAL_STATE, resp);
    };

    m_requestManager->sendRequest(m_statSocket, CMD_STATE_GET_BATTERY, QByteArray(), batteryCallback, 5000);
    m_requestManager->sendRequest(m_statSocket, CMD_STATE_GET_POSITION, QByteArray(), positionCallback, 5000);
    m_requestManager->sendRequest(m_statSocket, CMD_STATE_GET_NAVIGATIONAL_STATE, QByteArray(), navCallback, 5000);

    // 设置超时
    QObject::connect(&timeoutTimer, &QTimer::timeout, [&loop, &timeoutOccurred]() {
        timeoutOccurred = true;
        loop.quit();
    });
    timeoutTimer.start(5000);

    // 等待所有请求完成或超时
    loop.exec();

    if (timeoutOccurred) {
        LOG_ERROR("请求超时");
        return false;
    }

    // 检查所有请求是否成功完成
    bool allSuccess = true;
    for (const auto& pair : requestStatuses) {
        if (!pair.second.completed || !pair.second.success) {
            allSuccess = false;
            LOG_DEBUG("请求未完成或失败 - 命令ID: {}", pair.first);
        }
    }

    LOG_DEBUG("所有机器人状态获取完成 - 结果: {}", allSuccess);
    return allSuccess;
}

int main(int argc, char *argv[])
{
    // 初始化全局日志记录器
    GlobalLogger::getInstance().initialize("data/logs", "RMSClient_status", 5);
    
    // 初始化Qt应用
    QCoreApplication app(argc, argv);
    
    rclcpp::init(argc, argv);
    auto node = std::make_shared<RobotStatusService>();
    
    // 使用单线程执行器处理ROS 2回调
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    
    // 创建一个QTimer来定期调用executor的spin_some
    QTimer timer;
    timer.setInterval(10); // 10ms
    QObject::connect(&timer, &QTimer::timeout, [&executor]() {
        executor.spin_some();
    });
    timer.start();
    
    // Qt事件循环在主线程中运行，同时定期处理ROS 2回调
    int result = app.exec();
    
    rclcpp::shutdown();
    return result;
}

#include "robot_status_server.moc"